// Copyright (c) 2016-2017, XMOS Ltd, All rights reserved
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <stdint.h>
#include <stddef.h>
#include <limits.h>
#include <ctype.h>
#include <errno.h>
#include <stdint.h>
#if !(defined(_MSC_VER) || defined(__arm__))
#include <readline/readline.h>
#include <readline/history.h>
#endif
#include <control_host.h>
#include "signals.h"
#include <math.h>
#include "descriptions.h"
#include "xmos_baidu_control.h"

// For sleep functions
#if defined(_MSC_VER)
#include <windows.h>
#else
#include <unistd.h>
#endif

#if USE_XSCOPE
// xSCOPE requires an IP address and port number to connect
#define EXTRA_XSCOPE_ARGS 2
#else
#define EXTRA_XSCOPE_ARGS 0
#endif

int num_params;

// The three functions below were written for non-c99 compilers
cmdspec_t cmd_con(param_func pfunc,
                  char par_name[30],
                  param_type type,
                  unsigned offset,
                  int_float max,
                  int_float min,
                  param_rw rw,
                  char info[200])
{
  cmdspec_t cmd;
  cmd.pfunc = pfunc;
  strcpy(cmd.par_name, par_name);
  cmd.type = type;
  cmd.offset = offset;

  if(type == TYPE_FLOAT) {
    cmd.max.f = max.f;
    cmd.min.f = min.f;
  } else {
    cmd.max.i = max.i;
    cmd.min.i = min.i;
  }

  cmd.rw = rw;
  strcpy(cmd.info, info); 
  return cmd;
}

int_float cmd_f(float val)
{
  int_float f;
  f.f = val;
  return f;
}

int_float cmd_i(int val)
{
  int_float i;
  i.i = val;
  return i;
}

char * convert(char * s, float x)
{
    char buf[100];
    char *p;

    sprintf(buf, "%f", x);
    p = buf + strlen(buf) - 1;
    while (*p == '0' && *p-- != '.');
    *(p+1) = '\0';
    if (*p == '.') *p = '\0';
    strcpy(s, buf);
    return s;
}

void sleep_milli(unsigned milliseconds) {
#if defined(_MSC_VER)
  Sleep(milliseconds);
#else
  usleep(milliseconds * 1000);
#endif
}

int get_cmdspec_num(cmdspec_t cmdspec[], char* field)
{
  for(int i=0; i<num_params; i++) {
    if(strcmp(field, cmdspec[i].par_name) == 0) {
      return i;
    }
  }

  return -1;
}

void set_struct_val_on_device(cmdspec_t current, int_float struct_val)
{
  unsigned payload[3];
  
  memcpy(payload, &current.offset, sizeof(unsigned));
  memcpy(&(payload[1]), &struct_val, sizeof(unsigned));
  memcpy(&(payload[2]), &(current.type), sizeof(unsigned));
  
  unsigned resource = (current.pfunc == DOA) ? RESOURCE_ID_DOA : RESOURCE_ID_OTHER;

  if(current.pfunc != BOTH) {
    if (control_write_command(resource, CONTROL_CMD_SET_WRITE(0), (unsigned char *) payload, sizeof(int) * 3) != CONTROL_SUCCESS) {
      fprintf(stderr, "Error: Control write command failed\n");
      exit(1);
    }
  } else {
    if (control_write_command(RESOURCE_ID_DOA, CONTROL_CMD_SET_WRITE(0), (unsigned char *) payload, sizeof(int) * 3) != CONTROL_SUCCESS) {
      fprintf(stderr, "Error: Control write command failed\n");
      exit(1);
    }
    if (control_write_command(RESOURCE_ID_OTHER, CONTROL_CMD_SET_WRITE(0), (unsigned char *) payload, sizeof(int) * 3) != CONTROL_SUCCESS) {
      fprintf(stderr, "Error: Control write command failed\n");
      exit(1);
    }
  }
}

#if 0
beclear_version get_beclear_version(void) {
  unsigned payload[1] = {0xffffffff};
  if (control_read_command(RESOURCE_ID_AEC, CONTROL_CMD_SET_READ(BECLEAR_VERSION_CMD), (unsigned char *) payload, sizeof(unsigned)) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control read command failed\n");
    exit(1);
  }
  
  if(payload[0] != 0xffffffff) {
    return (beclear_version) payload[0];
  } else {
    fprintf(stderr, "Error: Device didn't write to payload\n");
    exit(1);
  }
}
#endif

int_float get_struct_val_from_device(cmdspec_t current)
{
  int payload[2];
  int_float return_val;
  unsigned char cmd = CONTROL_CMD_SET_READ((unsigned char) current.offset);
  cmd = (current.type == TYPE_INT) ? (cmd | 0x40) : cmd;
  unsigned resource = (current.pfunc == DOA) ? RESOURCE_ID_DOA : RESOURCE_ID_OTHER;

  if (control_read_command(resource, cmd, (unsigned char *) payload, sizeof(int) * 2) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control read command failed\n");
    exit(1);
  }

  if(current.type == TYPE_INT) {
    memcpy(&return_val, payload, sizeof(unsigned));
  } else {
    float converted_float = payload[0] * powf(2, payload[1]);
    memcpy(&return_val, &converted_float, sizeof(unsigned));
  }
  return return_val;
}

void print_intfloat(cmdspec_t cmdspec, int_float print_me) {
  if(cmdspec.type == TYPE_FLOAT) {
    char bufferino[30];
    printf("%s", convert(bufferino, print_me.f));
  } else {
    printf("%i", print_me.i);
  }
}

// Emits all commands with delays between for device to respond
void test_run(cmdspec_t cmdspec[], unsigned num_params)
{
  printf("NUMPARAMS %d\n", num_params);

  int_float result;
  for(unsigned i=0; i<num_params; ++i) {

    printf("RESULT %s ", cmdspec[i].par_name);

    switch(cmdspec[i].rw) {
      case WRITE:
        printf("WRITE BLANK\n");
        set_struct_val_on_device(cmdspec[i], cmdspec[i].min); sleep_milli(16);
        continue;

      case READ:
        printf("READ ");
        break;

      case READ_WRITE:
        printf("READ_WRITE ");
        break;
    }
    
    print_intfloat(cmdspec[i], cmdspec[i].min); printf(":");
    print_intfloat(cmdspec[i], cmdspec[i].max); printf(":");

    result = get_struct_val_from_device(cmdspec[i]); sleep_milli(16);
    print_intfloat(cmdspec[i], result);

    if(cmdspec[i].rw == READ_WRITE) {
      printf(":");

      set_struct_val_on_device(cmdspec[i], cmdspec[i].min); sleep_milli(16);
      result = get_struct_val_from_device(cmdspec[i]); sleep_milli(16);
      print_intfloat(cmdspec[i], result); printf(":");

      set_struct_val_on_device(cmdspec[i], cmdspec[i].max); sleep_milli(16);
      result = get_struct_val_from_device(cmdspec[i]); sleep_milli(16);
      print_intfloat(cmdspec[i], result);
    }

    printf("\n");
  }
}

#if USE_XSCOPE
#include "xscope_endpoint.h"
void shutdown(void)
{
  control_cleanup_xscope();
  exit(0);
}

void xscope_setup(char *ip_address, char *port)
{
  control_version_t version;

  signals_init();
  signals_setup_int(shutdown);

  if (control_init_xscope(ip_address, port) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control initialisation over xScope failed\n");
    exit(1);
  }

  if (control_query_version(&version) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control query version failed\n");
    exit(1);
  }
  
  if (version != CONTROL_VERSION) {
    fprintf(stderr, "Error: Mismatch of the control version between host and device. Expected 0x%X, received 0x%X\n", CONTROL_VERSION, version);
  }
}

#elif USE_USB
void shutdown(void)
{
  control_cleanup_usb();
  exit(0);
}

void usb_setup(void)
{
  control_version_t version;

  signals_init();
  signals_setup_int(shutdown);

  if(control_init_usb(0x20b1, 0x0011, 3) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control initialisation over USB failed\n");
    exit(1);
  }

  if (control_query_version(&version) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control query version failed\n");
    exit(1);
  }
  
  if (version != CONTROL_VERSION) {
    fprintf(stderr, "Error: Mismatch of the control version between host and device. Expected 0x%X, received 0x%X\n", CONTROL_VERSION, version);
    exit(1);
  }
}

#elif USE_I2C
void shutdown(void)
{
  control_cleanup_i2c();
  exit(0);
}

void i2c_setup(void)
{
  signals_init();
  signals_setup_int(shutdown);

  if (control_init_i2c(0x2c << 1) != CONTROL_SUCCESS) { //I2C address needs left shifting by one
    fprintf(stderr, "Error: Control initialisation over I2C failed\n");
    exit(1);
  }

#ifndef RPI
  control_version_t version;
  if (control_query_version(&version) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control query version failed\n");
    exit(1);
  }

  if (version != CONTROL_VERSION) {
    fprintf(stderr, "Error: Mismatch of the control version between host and device. Expected 0x%X, received 0x%X\n", CONTROL_VERSION, version);
  }
#endif // !RPI
}

#else // USE_SPI
void shutdown(void)
{
  control_cleanup_spi();
  exit(0);
}

void spi_setup(void)
{
  signals_init();
  signals_setup_int(shutdown);

  if (control_init_spi(3, 125, 1000) != CONTROL_SUCCESS) {
    fprintf(stderr, "Error: Control initialisation over SPI failed\n");
    exit(1);
  }
}
#endif

void print_help()
{
  printf("Usage:\n\n");
  printf(" <parameter>\t\tReturn the current value of <parameter>\n");
  printf(" <parameter> <value>\tAssign <value> to <parameter>\n");
  printf(" help (h)\t\tDisplay this information\n");
  printf(" help (h) <parameter>\tDisplay information for <parameter>\n");
  printf(" parameters (p)\t\tDisplay the list of parameters\n");
  printf("\n");
}

void print_headers(void)
{
  printf("%-22s %-7s %-12s %-12s %-12s %-40s\n", "paramater", "type", "max", "min", "r/w", "info");
  printf("%-22s %-7s %-12s %-12s %-12s %-40s\n", "---------", "----", "---", "---", "---", "----");
}

void print_line(cmdspec_t c)
{
  printf("%-22s ", c.par_name);

  if(c.type == TYPE_INT) {
    printf("%-7s %-12d %-12d", "int", c.max.i, c.min.i);
  } else {
    char buffer[2][30];
    printf("%-7s %-12s %-12s", "float", convert(buffer[0], c.max.f), convert(buffer[1], c.min.f));
  }

  if(c.rw == READ)        printf(" %-12s", "read-only");
  else if (c.rw == WRITE) printf(" %-12s", "write-only");
  else                    printf(" %-12s", "read-write");
  char* info = c.info;
  int count = 0;
  int firstLine = 0;

  while(info[count] != '\0')
  {
    if(info[count] == '\n')
    { 
      if(firstLine == 0)
      {
        printf(" %.*s\n" ,count,info);
        firstLine = 1;
      }
      else
      {
        printf("%70s%.*s\n"," ",count,info);
      }
      info += count+1;
      count =0;
    }
    else{
      count++;
    }
  }

  if(firstLine == 0)
  {
	printf(" %.*s\n" ,count,info); //if its still the first line
	firstLine = 1;
  }
  else
  {
    printf("%70s%.*s\n"," ",count,info); //if its another added line
  }
}

void print_parameters(cmdspec_t cmdspec[], unsigned num_params)
{
  print_headers();
  
  for(int i=0; i<num_params; i++)
    print_line(cmdspec[i]);

  printf("\n");
}

void print_usage(const char *arg0, cmdspec_t cmdspec[], unsigned num_cmds)
{
  printf("Interactive usage: %s\n", arg0);
  printf("Scripting usage: %s PARAMETER [VALUE]\n\n", arg0);
  printf("commands for %s:\n","BaiduMIcArray");
  print_parameters(cmdspec, num_cmds);
}

// NUM_PARAMS_SMARTTV > NUM_PARAMS_SMARTHOME
char completes[NUM_PARAMS_BAIDU][30];

void fill_completes(cmdspec_t cmdspec[], unsigned num_params)
{
  for(unsigned i=0; i<num_params; ++i) {
    strcpy(completes[i], cmdspec[i].par_name);
  }
}

char* my_generator(const char* text, int state)
{
  static int list_index, len;
  char * found;

  if (!state) {
    list_index = -1;
    len = strlen(text);
  }

  while (list_index < num_params-1) {
    list_index++;
    if (strncmp (completes[list_index], text, len) == 0) {
      found = (char *)malloc(len + 1);
      strcpy(found, completes[list_index]);
      return found;
    }
  }

  return ((char *) NULL);
}

#if !(defined(_MSC_VER) || defined(__arm__))
char** completion(const char* text, int start, int end)
{
  char ** matches = (char **) NULL;
  if(start == 0) {
    matches = (char **) rl_completion_matches(text, &my_generator);
  } else {
    /* Disable filename autocompletion */
    rl_attempted_completion_over = 1;
  }
  return matches;
}
#endif

int check_and_do_command(char * par_name, cmdspec_t *cmdspec, int num_args, char * struct_val_string){
  int cmdspec_num;
  cmdspec_t current;
  int_float struct_val;

  if(!strlen(par_name) || num_args == -1) {
    // Skip
    return 0;
  }

  if(strcmp(par_name, "help") == 0 ||
     strcmp(par_name, "h") == 0)
  {
    cmdspec_num = get_cmdspec_num(cmdspec, struct_val_string);
    
    if (cmdspec_num != -1) {
      print_headers();
      print_line(cmdspec[cmdspec_num]);
    } else {
      print_help();
    }
    return 0;
  }

  if(strcmp(par_name, "parameters") == 0 ||
     strcmp(par_name, "p") == 0) {
    print_parameters(cmdspec, num_params);
    return 0;
  }



  cmdspec_num = get_cmdspec_num(cmdspec, par_name);
  if (cmdspec_num == -1) {
    return -1;
  }
  current = cmdspec[cmdspec_num];

  if(current.type == TYPE_INT) {
    sscanf(struct_val_string, "%d", &(struct_val.i));
  } else {
    sscanf(struct_val_string, "%f", &(struct_val.f));
  }

  if(num_args == 2 &&
     current.rw == READ) {
    printf("Cannot write to read-only parameter\n");
    return -2;
  }

  if(num_args == 1 &&
     current.rw == WRITE) {
    printf("Cannot read from write-only parameter\n");
    return -3;
  }

  if (num_args == 2) {
    if(current.type == TYPE_INT) {
      if(struct_val.i > current.max.i) {
        printf("Value given is greater than the max specified: %d > %d.\n", struct_val.i, current.max.i);
        return -4;
      }
      if(struct_val.i < current.min.i) {
        printf("Value given is less than the min specified: %d < %d.\n", struct_val.i, current.min.i);
        return -5;
      }
    } else { // TYPE_FLOAT
      if(struct_val.f > current.max.f) {
        printf("Value given is greater than the max specified: %f > %f.\n", struct_val.f, current.max.f);
        return -6;
      }
      if(struct_val.f < current.min.f) {
        printf("Value given is less than the min specified: %f < %f.\n", struct_val.f, current.min.f);
        return -7;
      }
    }
  }

  // Get or set result
  if (num_args == 2) {
    set_struct_val_on_device(current, struct_val);
  } else {
    struct_val = get_struct_val_from_device(current);
  }

  // Print result
  printf("%s:", par_name);
  if(current.type == TYPE_FLOAT) {
    char bufferino[30];
    printf("%s\n", convert(bufferino, struct_val.f));
  } else {
    printf("%i\n", struct_val.i);
  }
  return 0;
}

int main(int argc, char *argv[])
{
  cmdspec_t cmdspec_baidu[] = {
  // pfunc         par_name                 type          offset                          max               min             r/w/rw       info
    cmd_con(DOA,   "ANGLE",        					TYPE_INT,   	BAIDU_DOA_ANGLE,       					cmd_i(360),    		cmd_i(0),    		READ_WRITE,  DOA_ANGLE_DESC     )
  };
  

#if USE_XSCOPE
  if (argc >= (1+EXTRA_XSCOPE_ARGS)) {
    char *xscope_ip = argv[1];
    char *xscope_port = argv[2];
    xscope_setup(xscope_ip, xscope_port);
  }
  else {
    printf("When built with \"USE_XSCOPE\" it must be run as \"%s <xSCOPE IP address> <xSCOPE port>...\"\n", argv[0]);
    exit(1);
  }
#elif USE_USB
  usb_setup();
#elif USE_I2C
  i2c_setup();
#else
  spi_setup();
#endif

  cmdspec_t * cmdspec;
  
  cmdspec = cmdspec_baidu;
  num_params = NUM_PARAMS_BAIDU;

  // Cases for --help, test mode or script mode read
  if(argc == (2+EXTRA_XSCOPE_ARGS)){
    if(strcmp(argv[1+EXTRA_XSCOPE_ARGS], "test") == 0) {
      test_run(cmdspec, num_params);
    }
    else if ((strcmp(argv[1+EXTRA_XSCOPE_ARGS], "--help") == 0) || (strcmp(argv[1+EXTRA_XSCOPE_ARGS], "-h") == 0)) {
      print_usage(argv[0], cmdspec, num_params);
    }
    else { // we are reading a parameter
      int result;
      if ((result = check_and_do_command(argv[1+EXTRA_XSCOPE_ARGS], cmdspec, argc - 1, "")) != 0){
      printf("Failure on read command command: %s error code: %d\n", argv[1+EXTRA_XSCOPE_ARGS], result);
      printf("Type \"%s --help\" for usage\n", argv[0]);
      return result;
      }
    }
    return 0;
  }

  //Test for scripting mode write and do if so
  if (argc == (3+EXTRA_XSCOPE_ARGS)) {
    int result;
    if ((result = check_and_do_command(argv[1+EXTRA_XSCOPE_ARGS], cmdspec, argc - 1, argv[2])) != 0){
      printf("Failure on write command: %s %s error code: %d\n", argv[1+EXTRA_XSCOPE_ARGS], argv[+EXTRA_XSCOPE_ARGS], result);
      printf("Type \"%s --help\" for usage\n", argv[0]);
      return result;
    }
    return 0;
  }

  //We drop through to interactive mode
  printf("Connected to a %s device.\n", "BaiduMicArray");


  /* Enable auto-complete
   * Currently disabled for Windows/RPI builds */
#if !(defined(_MSC_VER) || defined(__arm__))
  fill_completes(cmdspec, num_params);
  rl_attempted_completion_function = completion;
  rl_bind_key('\t', rl_complete);
#endif

  int num_args;
  int foreign_keys = 0;
  char par_name[100];
  char struct_val_string[20];
  char * buffer;

  print_help();

  while (1) {
    foreign_keys = 0;
    par_name[0] = '\0';
    struct_val_string[0] = '\0';

#if defined(_MSC_VER) || defined(__arm__)
    buffer = (char *) malloc(120);
    fgets(buffer, 120, stdin);
#else // OSX
    buffer = readline("");
    if(!buffer) continue;
    if(*buffer) add_history(buffer);
#endif
    num_args = sscanf(buffer, "%s %s", par_name, struct_val_string);
    free(buffer);

    // Remove trailing \n from buffer
    par_name[strcspn(par_name, "\r\n")] = 0;
    struct_val_string[strcspn(struct_val_string, "\r\n")] = 0;

#if defined(_MSC_VER) || defined(__arm__)
    // Check par_name is alphanumeric
    int struct_length = strlen(par_name);
    for(int i=0; i<struct_length; i++) {
      if(!isalnum(par_name[i]) && 
         par_name[i] != '_') {
        foreign_keys = 1;
        break;
      }
    }
    // Check struct_val is a float/int
    if(num_args == 2) {
      struct_length = strlen(struct_val_string);
      for(int i=0; i<struct_length; i++) {
        if(!isalnum(struct_val_string[i]) && 
            struct_val_string[i] != '.' &&
            struct_val_string[i] != '-' &&
            struct_val_string[i] != '_') {
          foreign_keys = 1;
          break;
        }
      }
    }

    if(foreign_keys) {
      fprintf(stderr, "Foreign keys found in input\n");
      continue;
    }
#endif
    
    if (check_and_do_command(par_name, cmdspec, num_args, struct_val_string) != 0){
      printf("Unrecognised parameter \"%s\". Enter \"parameters\" to see available parameters\n", par_name);
    }

  
  } // while

  return 0;
}
